/*
 * opxx.c
 *
 * Copyright (c) 2009, Eugen Stoian <stoian.e@gmail.com>
 *
 * This library is free software; you can redistribute it and/or modify it under
 * the terms of the GNU Lesser General Public License as published by the Free
 * Software Foundation; either version 3 of the License, or (at your option)
 * any later version.
 *
 * This library is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 * FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
 * details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this library; if not, write to the Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA
 * http://www.fsf.org/licensing/licenses/lgpl.txt
 */

#include <opsbase.h>
#include <pxxdefs.h>
#include <xmmintrin.h>


void v_addv_ps(int n, float const * op1, float const * op2, float * result) {
	int i = 0;
	for (; i < n; i += 4) {
		__m128 r1 = _mm_load_ps(op1 + i);
		__m128 r2 = _mm_load_ps(op2 + i);
		__m128 r = _mm_add_ps(r1, r2);
		_mm_store_ps(result + i, r);
	}
}
void v_subv_ps(int n, float const * op1, float const * op2, float * result) {
	int i = 0;
	for (; i < n; i += 4) {
		__m128 r1 = _mm_load_ps(op1 + i);
		__m128 r2 = _mm_load_ps(op2 + i);
		__m128 r = _mm_sub_ps(r1, r2);
		_mm_store_ps(result + i, r);
	}
}
void v_mulv_ps(int n, float const * op1, float const * op2, float * result) {
	int i = 0;
	for (; i < n; i += 4) {
		__m128 r1 = _mm_load_ps(op1 + i);
		__m128 r2 = _mm_load_ps(op2 + i);
		__m128 r = _mm_mul_ps(r1, r2);
		_mm_store_ps(result + i, r);
	}
}
void v_divv_ps(int n, float const * op1, float const * op2, float * result) {
	int i = 0;
	for (; i < n; i += 4) {
		__m128 r1 = _mm_load_ps(op1 + i);
		__m128 r2 = _mm_load_ps(op2 + i);
		__m128 r = _mm_div_ps(r1, r2);
		_mm_store_ps(result + i, r);
	}
}

void v_muls_ps(int n, float const * op1, float op2, float * result) {
	int i = 0;
	__m128 r0 = _mm_set1_ps(op2);
	for (; i < n; i += 4) {
		__m128 r1 = _mm_load_ps(op1 + i);
		__m128 r2 = _mm_mul_ps(r0, r1);
		_mm_store_ps(result + i, r2);
	}
}

void v_divs_ps(int n, float const * op1, float op2, float * result) {
	int i = 0;
	__m128 r0 = _mm_set1_ps(op2);
	__m128 r1 = _mm_set1_ps(1.0f);
	r1 = _mm_div_ps(r1,r0);
	for (; i < n; i += 4) {
		__m128 r2 = _mm_load_ps(op1 + i);
		__m128 r3 = _mm_mul_ps(r1, r2);
		_mm_store_ps(result + i, r3);
	}
}

void v_muls_addv_ps(int n, float const * op1, float op2, float const * op3,
		float * result) {
	int i = 0;
	__m128 r0 = _mm_set1_ps(op2);
	for (; i < n; i += 4) {
		__m128 r1 = _mm_load_ps(op1 + i);
		__m128 r2 = _mm_load_ps(op3 + i);
		__m128 r3 = _mm_mul_ps(r0, r1);
		__m128 r4 = _mm_add_ps(r2, r3);
		_mm_store_ps(result + i, r4);
	}
}

float v_dotv_ps(int n, float const * op1, float const * op2) {
	float sum = 0.0f;
	int i = 0;
	__m128 r0 = _mm_set_ss(sum);
	for (; i < n; i += 4) {
		__m128 r1 = _mm_load_ps(op1 + i);
		__m128 r2 = _mm_load_ps(op2 + i);
		__m128 r3 = _mm_mul_ps(r1, r2);
		r0 = _mm_add_ps(r0, r3);
	}
	__m128 s0 = _mm_shuffle_ps(r0, r0, _MM_SHUFFLE(1, 0, 3, 2));
	__m128 s1 = _mm_add_ps(r0, s0);
	__m128 s2 = _mm_shuffle_ps(s1, s1, _MM_SHUFFLE(0, 1, 2, 3));
	__m128 s3 = _mm_add_ps(s1, s2);
	float result;
	_mm_store_ss(&result, s3);
	return result;
}

void v_addv_pd(int n, double const * op1, double const * op2, double * result) {
	int i = 0;
	for (; i < n; i += 2) {
		__m128d r1 = _mm_load_pd(op1 + i);
		__m128d r2 = _mm_load_pd(op2 + i);
		__m128d r = _mm_add_pd(r1, r2);
		_mm_store_pd(result + i, r);
	}
}

void v_subv_pd(int n, double const * op1, double const * op2, double * result) {
	int i = 0;
	for (; i < n; i += 2) {
		__m128d r1 = _mm_load_pd(op1 + i);
		__m128d r2 = _mm_load_pd(op2 + i);
		__m128d r = _mm_sub_pd(r1, r2);
		_mm_store_pd(result + i, r);
	}
}

void v_mulv_pd(int n, double const * op1, double const * op2, double * result) {
	int i = 0;
	for (; i < n; i += 2) {
		__m128d r1 = _mm_load_pd(op1 + i);
		__m128d r2 = _mm_load_pd(op2 + i);
		__m128d r = _mm_mul_pd(r1, r2);
		_mm_store_pd(result + i, r);
	}
}

void v_divv_pd(int n, double const * op1, double const * op2, double * result) {
	int i = 0;
	for (; i < n; i += 2) {
		__m128d r1 = _mm_load_pd(op1 + i);
		__m128d r2 = _mm_load_pd(op2 + i);
		__m128d r = _mm_div_pd(r1, r2);
		_mm_store_pd(result + i, r);
	}
}

void v_muls_pd(int n, double const * op1, double op2, double * result) {
	int i = 0;
	__m128d r0 = _mm_set1_pd(op2);
	for (; i < n; i += 2) {
		__m128d r1 = _mm_load_pd(op1 + i);
		__m128d r2 = _mm_mul_pd(r0, r1);
		_mm_store_pd(result + i, r2);
	}
}

void v_divs_pd(int n, double const * op1, double op2, double * result) {
	int i = 0;
	__m128d r0 = _mm_set1_pd(op2);
	__m128d r1 = _mm_set1_pd(1.0);
	r1 = _mm_div_pd(r1,r0);
	for (; i < n; i += 2) {
		__m128d r2 = _mm_load_pd(op1 + i);
		__m128d r3 = _mm_mul_pd(r1, r2);
		_mm_store_pd(result + i, r3);
	}
}

double v_dotv_pd(int n, double const * op1, double const * op2) {
	double sum = 0.0;
	int i = 0;
	__m128d r0 = _mm_set_sd(sum);
	for (; i < n; i += 2) {
		__m128d r1 = _mm_load_pd(op1 + i);
		__m128d r2 = _mm_load_pd(op2 + i);
		__m128d r3 = _mm_mul_pd(r1, r2);
		r0 = _mm_add_pd(r0, r3);
	}
	__m128d s0 = _mm_shuffle_pd(r0, r0, 1);
	__m128d s1 = _mm_add_pd(r0, s0);
	double result;
	_mm_store_sd(&result, s1);
	return result;
}

void v_muls_addv_pd(int n, double const * op1, double op2, double const * op3,
		double * result) {
	int i = 0;
	__m128d r0 = _mm_set1_pd(op2);
	for (; i < n; i += 2) {
		__m128d r1 = _mm_load_pd(op1 + i);
		__m128d r2 = _mm_load_pd(op3 + i);
		__m128d r3 = _mm_mul_pd(r0, r1);
		__m128d r4 = _mm_add_pd(r2, r3);
		_mm_store_pd(result + i, r4);
	}
}
